#include "body.h"

using namespace sde;

Body::Body(std::string _type, std::string _name, Object* _object) :
    Action("Body",_type,_name,_object),
    physicsServer(0),
    dynamicsWorld(0),
    collisionShape(0),
    motionState(0),
    rigidBody(0) {
    initDepend = "physics/PhysicsServer";
}

void Body::run() {
    physicsServer = (PhysicsServer*)Action::getFirstActionOfArchetype("physics/PhysicsServer");
    if (physicsServer) {
        dynamicsWorld = physicsServer->getDynamicsWorld();
        collisionShape = makeCollisionShape();
        
        if (collisionShape) {
            Quaternion rotation = boost::any_cast<Quaternion>(object->getVar("rotation",Quaternion::IDENTITY));
            Vector3 position = boost::any_cast<Vector3>(object->getVar("position",Vector3::ZERO));
            motionState = new BodyMotionState(object,btTransform(PhysicsServer::QuaternionToBulletQuaternion(rotation),
                PhysicsServer::VectorToBulletVector(position)));
                
            float mass = boost::any_cast<float>(object->getVar("mass",float(1)));
            btVector3 inertia;
            collisionShape->calculateLocalInertia(mass,inertia);
            
            btRigidBody::btRigidBodyConstructionInfo rbci(mass,motionState,collisionShape,inertia);
            rigidBody = new btRigidBody(rbci);
            dynamicsWorld->addRigidBody(rigidBody);
        }
    }
}

void Body::stop() {
    if (dynamicsWorld && rigidBody) {
        dynamicsWorld->removeRigidBody(rigidBody);
        delete rigidBody->getMotionState();
        delete rigidBody;
        delete collisionShape;
    }
}
